import rospy
from sensor_msgs.msg import Joy
from rovmsg.msg import rovmsg
import subprocess


class Controller:
    def __init__(self):

        rospy.init_node("rov_cmd")
        self.ROBOT = rospy.get_param("~robot")
        self.DEVICE = rospy.get_param("~device")
        
        self.set_param = subprocess.Popen(["rosparam","set","/joy_node/dev","'/dev/input/{}'".format(self.DEVICE)])
        self.joy_node = subprocess.Popen(["rosrun","joy","joy_node"])
        self.pub = rospy.Publisher("{}/cmd_vel".format(self.ROBOT),rovmsg,queue_size=10)
        rospy.Subscriber("/joy",Joy,self.call_back)
        
        self.v_param = 4.0
        self.lz_param = 1
        
        self.pitch_param = 3.14
        self.roll_param = 3.14
        self.yaw_param = 3.14

        self.key = 3
        self.light = 0
        self.mode = {}
        self.mode["3"] = 1
        self.mode["1"] = 2
        self.mode["0"] = 3
        self.mode["2"] = 4
        self.last_axes7 = 0
        rospy.spin()
    
    def call_back(self,data):
        msg = rovmsg()
        # move
        msg.twist.linear.x = self.v_param*data.axes[1]
        msg.twist.linear.y = self.v_param*data.axes[0]
        msg.twist.linear.z = self.lz_param*(-0.5*(data.axes[2]-1)+0.5*(data.axes[5]-1))
        # turn
        if data.buttons[4] == 1:
            msg.twist.angular.x = self.roll_param*data.axes[3]
            msg.twist.angular.y = self.pitch_param*data.axes[4]
            msg.twist.angular.z = 0
        else:
            msg.twist.angular.x = 0
            msg.twist.angular.y = 0
            msg.twist.angular.z = self.yaw_param*data.axes[3]
        # mode change
        try:
            self.key = data.buttons.index(1)
        except:
            pass
        if str(self.key) in self.mode.keys():
            msg.mode = self.mode[str(self.key)]

        # light change
        if self.last_axes7 == 0:
            self.light = self.light+data.axes[7]
        self.light = 0 if self.light<0 else self.light
        msg.light = int(self.light)
        self.last_axes7 = data.axes[7]
        rospy.loginfo("test")
        self.pub.publish(msg)

if __name__ == "__main__":
    controller = Controller()